{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "#Global Configurations\n",
    "import sys\n",
    "sys.path.append('/opt/ros/noetic/lib/python3/dist-packages/')\n",
    "global_rosbag_path = '/home/xuhao/data/camera190_closer/camera190_closer.bag'\n",
    "global_output_path = '/home/xuhao/data/camera190_closer/'\n",
    "\n",
    "global_photometric_calibration_path = None\n",
    "\n",
    "global_image_topic = '/oak_ffc_4p/assemble_image/compressed'\n",
    "global_imu_topic = '/uav3/mavros/imu/data_raw'\n",
    "\n",
    "global_virtual_fov = 190\n",
    "global_virtual_width = 320\n",
    "global_virtual_height = 240"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Split bag into 9 parts for calibration maybe slow if bag is large\n",
    "import utils.Utils as Utils\n",
    "import utils.BagExtractor as BagExtractor\n",
    "\n",
    "ret = Utils.check_rosbag(global_imu_topic,global_image_topic,global_rosbag_path)\n",
    "if ret == False:\n",
    "  exit(1)\n",
    "bags = BagExtractor.extract_calibration_bag(global_rosbag_path,global_output_path, global_image_topic)\n",
    "print(\"[Bag extractor] The following bags are created:\\n\")\n",
    "for bag_file in bags:\n",
    "  print(bag_file)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Calibrate each fisheye camera \n",
    "import utils.Calibration as Calibration\n",
    "\n",
    "Calibration.calibrate_monos(global_output_path)\n",
    "Calibration.check_calibration(global_output_path,\"mono\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# # Result of this cell is temporary unused in the following cells\n",
    "# import utils.Calibration as Calibration\n",
    "\n",
    "# Calibration.calibrate_stereos(global_output_path)\n",
    "# Calibration.check_calibration(global_output_path,\"stereo\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import utils.Calibration as Calibration\n",
    "Calibration.calibrate_cam_imu(global_output_path)\n",
    "Calibration.check_imu_calibration(global_output_path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import utils.Utils as Utils\n",
    "Utils.generate_d2vins_cinfiguration(global_output_path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import utils.VirtualStereoCalibration as VirtualStereoCalibration\n",
    "input_bag = global_output_path + \"/stereo_depth_calibration.bag\"\n",
    "output_path = global_output_path + f\"/virtual_stereo_calibration_{global_virtual_fov}\"\n",
    "fisheye_config = global_output_path+ \"fisheye_cams.yaml\"\n",
    "step = 1 \n",
    "VirtualStereoCalibration.calibrate_virtual_stereo(input_bag,  global_virtual_fov, \n",
    "                                                  global_virtual_width, global_virtual_height,\n",
    "                                                  fisheye_config, output_path, step, global_photometric_calibration_path,verbose=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
